function [x,P] = flowCorrect(x,P,y)
%FLOWCORRECT Correct state estimation with optical flow data
%
    persistent n; n =0.0625;
    persistent N fx fy; fx = 350; fy = 350; N = [n, 0; 0, n];
    
    if x(3) < 0.1
        height = 0.1;
    else
        height = x(3);
    end

    % measurement model
    h = [- fx*y(5) - (fx*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10)))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height;...
           fy*y(4) - (fy*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10)))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height];

    % h = [x(4); x(5)];

    % Jacobian of measurement model w.r.t error states
    H = [ 0, 0, (fx*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10)))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height^2, -(fx*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2))/height,         -(fx*(2*x(7)*x(10) + 2*x(8)*x(9))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height,  (fx*(2*x(7)*x(9) - 2*x(8)*x(10))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height, (x(8)*((2*fx*x(7)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height + (fx*(2*x(7)*x(4) - 2*x(9)*x(6) + 2*x(10)*x(5))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 + (x(7)*((2*fx*x(8)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height - (fx*(2*x(8)*x(4) + 2*x(9)*x(5) + 2*x(10)*x(6))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 + (x(10)*((2*fx*x(9)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height + (fx*(2*x(7)*x(6) - 2*x(8)*x(5) + 2*x(9)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 + (x(9)*((2*fx*x(10)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height + (fx*(2*x(7)*x(5) + 2*x(8)*x(6) - 2*x(10)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2, (x(7)*((2*fx*x(9)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height + (fx*(2*x(7)*x(6) - 2*x(8)*x(5) + 2*x(9)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 + (x(9)*((2*fx*x(7)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height + (fx*(2*x(7)*x(4) - 2*x(9)*x(6) + 2*x(10)*x(5))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 - (x(8)*((2*fx*x(10)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height + (fx*(2*x(7)*x(5) + 2*x(8)*x(6) - 2*x(10)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 - (x(10)*((2*fx*x(8)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height - (fx*(2*x(8)*x(4) + 2*x(9)*x(5) + 2*x(10)*x(6))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2, (x(10)*((2*fx*x(7)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height + (fx*(2*x(7)*x(4) - 2*x(9)*x(6) + 2*x(10)*x(5))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 - (x(7)*((2*fx*x(10)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height + (fx*(2*x(7)*x(5) + 2*x(8)*x(6) - 2*x(10)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 - (x(8)*((2*fx*x(9)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height + (fx*(2*x(7)*x(6) - 2*x(8)*x(5) + 2*x(9)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 + (x(9)*((2*fx*x(8)*(x(4)*(x(7)^2 + x(8)^2 - x(9)^2 - x(10)^2) + x(5)*(2*x(7)*x(10) + 2*x(8)*x(9)) - x(6)*(2*x(7)*x(9) - 2*x(8)*x(10))))/height - (fx*(2*x(8)*x(4) + 2*x(9)*x(5) + 2*x(10)*x(6))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2;...
          0, 0, (fy*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10)))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height^2,          (fy*(2*x(7)*x(10) - 2*x(8)*x(9))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height, -(fy*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2))/height, -(fy*(2*x(7)*x(8) + 2*x(9)*x(10))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height, (x(7)*((2*fy*x(8)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height - (fy*(2*x(7)*x(6) - 2*x(8)*x(5) + 2*x(9)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 + (x(8)*((2*fy*x(7)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height + (fy*(2*x(7)*x(5) + 2*x(8)*x(6) - 2*x(10)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 + (x(9)*((2*fy*x(10)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height - (fy*(2*x(7)*x(4) - 2*x(9)*x(6) + 2*x(10)*x(5))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 + (x(10)*((2*fy*x(9)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height - (fy*(2*x(8)*x(4) + 2*x(9)*x(5) + 2*x(10)*x(6))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2, (x(9)*((2*fy*x(7)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height + (fy*(2*x(7)*x(5) + 2*x(8)*x(6) - 2*x(10)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 - (x(10)*((2*fy*x(8)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height - (fy*(2*x(7)*x(6) - 2*x(8)*x(5) + 2*x(9)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 + (x(7)*((2*fy*x(9)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height - (fy*(2*x(8)*x(4) + 2*x(9)*x(5) + 2*x(10)*x(6))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 - (x(8)*((2*fy*x(10)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height - (fy*(2*x(7)*x(4) - 2*x(9)*x(6) + 2*x(10)*x(5))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2, (x(9)*((2*fy*x(8)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height - (fy*(2*x(7)*x(6) - 2*x(8)*x(5) + 2*x(9)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 + (x(10)*((2*fy*x(7)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height + (fy*(2*x(7)*x(5) + 2*x(8)*x(6) - 2*x(10)*x(4))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 - (x(7)*((2*fy*x(10)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height - (fy*(2*x(7)*x(4) - 2*x(9)*x(6) + 2*x(10)*x(5))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2 - (x(8)*((2*fy*x(9)*(x(5)*(x(7)^2 - x(8)^2 + x(9)^2 - x(10)^2) - x(4)*(2*x(7)*x(10) - 2*x(8)*x(9)) + x(6)*(2*x(7)*x(8) + 2*x(9)*x(10))))/height - (fy*(2*x(8)*x(4) + 2*x(9)*x(5) + 2*x(10)*x(6))*(x(7)^2 - x(8)^2 - x(9)^2 + x(10)^2))/height))/2];
 
    % H = [0 0 0 1 0 0 0 0 0; 0 0 0 0 1 0 0 0 0]; 

    % meas = [sign(y(8))*0.02; 0];

    % compute innovation and covariance
    z = y(8:9) - h;
    % z = meas - h;
    Z = H*P*H.' + N;
    % compute gain
    K = (P*H.')/Z;
    % compute errors
    dx = K*z;
    % inject errors
    x = injectErrors(x,dx);
    % update P
    P = (eye(9) - K*H)*P*(eye(9) - K*H).' + K*N*K.';

end

